/*
* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty.  In no event will the authors be held liable for any damages
* arising from the use of this software.
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
* 3. This notice may not be removed or altered from any source distribution.
*/

#ifndef B2_BODY_H
#define B2_BODY_H

#include "../Common/b2Math.h"
#include "../Collision/Shapes/b2Shape.h"

#include <memory>

class b2Fixture;
class b2Joint;
class b2Contact;
class b2Controller;
class b2World;
struct b2FixtureDef;
struct b2JointEdge;
struct b2ContactEdge;
struct b2ControllerEdge;

// A body definition holds all the data needed for construct a rigid body.
// you can safely re-use body definition.
struct b2BodyDef
{
	// You can user this to initialize the mass properties of the body.
	// If you prefer, You can set the mass properties after shapes have been added
	// using b2Body::SetMassFromShapes.
	b2MassData massData;

	// Use this to store application specific body data.
	void* userData;

	// The world position of the body.Avoid creating the body at the origin
	// Since this can lead to many overlapping shapes.
	b2Vec2 position;

	// The world angle of the body in radians.
	float32 angle;

	// The linear velocity of the body in world co-ordinates.
	b2Vec2 linearVelocity;

	// The angular  velocity of the body.
	float32 angularVelocity;

	// Linear damping is used to reduce the linear velocity. This dumping parameter can be larger than 1.0f.
	// but the dumping effect becomes sensitive to the time step when the dumping parameter is larger.
	float32 linearDamping;

	// Angular damping is used to reduce the angular velocity.This damping parameter can be larger than 1.0f.
	// but the damping effect becomes sensitive to the time step when the damping parameter is larger.
	float32 angularDamping;

	// Set the flag to false, if this body should never fall to sleep.Note than this increase CPU usage.
	bool allowSleep;

	// Is the body initially sleeping?
	bool isSleeping;
	
	// Should this body be prevented from rotation.Usefull for charactor.
	bool fixedRotation;

	// Is this a fast moving body than should prevent from tunneling through other moving bodies?
	// Note that: all bodies are prevented from tunning through static bodies.
	// @Warning: You should user this flag Separingly since it increases processing time.
	bool isBullet;
};

// A rigid body.These are created via b2World::CreateBody
class b2Body
{
public:
	// Create a fixture and attach it to this body.
	// @param def the fixture definition
	// @warning This function was locked during callbacks.
	b2Fixture* CreateFixture(const b2FixtureDef* def);

	// Destroy a fixture,this remove the fixture from the broadphase and therefore destroy any constacts 
	// assosiations to this fixture. All fixtures attached to a body were implicitly destroied when the body
	// is distroied.
	// @param fixture the fixture to be removed.
	// @warning This function was locked during callbacks.
	void DestroyFixtrue(b2Fixture* fixture);

	// Set the mass properties.Note this changes the center of the mass position.
	// If you are not sure how to compute mass properties.use SetMassFromShapes.
	// The inertia tensor is assumed to be relative to the center of the mass.
	// @param massData the mass properties.
	void SetMassData(const b2MassData* data);

	// Compute the mass properties from attached fixture.you typically call this after adding all the fixtures.
	// If you add or remove fixture later, you may want to call this again.Note,this change the mass position.
	void SetMassFromShapes();

	// Set the position of the body's origin and rotation(radians).
	// This breaks any contacts and wakes the other bodies.
	// @param position the new world position of the body's origin.
	// @angle the new world rotation angle of the body in radians.
	// @return false if the movement put a shape outside of the world.In this case the body is automaticlly frozen.
	bool SetXForm(const b2Vec2& position, float32 angle);

	// Set the position of the body's origin and rotation(radians).
	// This breaks any contacts and wakes the other bodies.
	/// @param xf the transform of position and angle to set the body to.
	/// @return false if the movement put a shape outside the world. In this case the
	/// body is automatically frozen.
	bool SetXForm(const b2XForm& xf) { return SetXForm(xf.position, xf.GetAngle()); }

	// Get the body transform for the body's origin.
	// @return the world tranform of the body's origin.
	const b2XForm& GetXForm() const { return m_xf; }

	// Set the world body origin position.
	// @param position the new position for the body.
	void SetPosition(const b2Vec2& position) { SetXForm(position, GetAngle()); }

	// Set the wold body angle.
	// @param angle the new angle of the body.
	void SetAngle(float32 angle) { SetXForm(GetPosition(), angle); }

	// Get the wold body position.
	// @return the wold body position of the origin.
	const b2Vec2& GetPosition() const { return m_xf.position; }

	// Get the angle in radians.
	// @return the current world rotation angle in randians.
	float32 GetAngle() const { return m_sweep.a; }

	// Get the world position of the center of mass.
	const b2Vec2& GetWorldCenter() const { return m_sweep.c; }

	// Get the local position of the center of mass.
	const b2Vec2& GetLocalCenter() const { return m_sweep.localCenter; }

	// Set the linear velocity of the center of the mass.
	// @param v the new linear velocity of the center of the mass.
	void SetLinearVelocity(const b2Vec2& v) { m_linearVelocity = v; }

	// Get the linear velocity of the center of the mass.
	b2Vec2 GetLinearVelocity() const { return m_linearVelocity; }

	// Set the angular velocity.
	// @return the angular velocity in radians/second.
	void SetAngularVelocity(float32 omega) { m_angularVelocity = omega; }

	// Get the angular velocity.
	// return the angular velocity in radians/second.
	float32 GetAngumarVelocity() const { return m_angularVelocity; }

	// Apply a force at a world position. If the force is not applied at the center of the mass.
	// it will generate a torque and affect the angular velocity.This wakes up the body.
	// @param force the world force vector. usually in Newtons(N)
	// @param point the world position of the point of application.
	void ApplyForce(const b2Vec2& force, const b2Vec2& point);

	// Apply a torque. This affect the angular velocity.without affecting the linear velocity of the center of the mass.
	// this wake up the body.
	// @param torque about the z-axis (out of screen.)usually in N-m
	void ApplyTorque(float32 torque);

	// Apply a Impulse at a point.This immediately modifies the velocity.
	// It also modify the angular velocity if the point of the application is
	// not at the center of the mass.This wake up the body.
	// @param impulse the world impulse vector.usually in N-seconds or kg-m/s
	// @param point the wold position of the point of application.
	void ApplyImpulse(const b2Vec2& impulse, const b2Vec2& point);

	// Get the total mass of the body.
	// @return the mass,usually in kg.
	float32 GetMass() const { return m_mass; }

	// Get the center rotation inertia of the body.
	// @return the rotation inertia. usually in kg-m^2.
	float32 GetInertia() const { return m_I; }

	// Get the mass data of the body.
	// @return a structure containing the mass , inertia ,and center of the body.
	b2MassData GetMassData() const;

	// Get the world coordinates of a point and given the local coordinates.
	// @param localPoint a point on the body measured relative the body's origin.
	// @return the same point expressed in world coordinates.
	b2Vec2 GetWorldPoint(const b2Vec2& localPoint) const { return b2Mul(m_xf, localPoint); }

	// Get the world coordinates of a vector and given the local coordinates.
	// @param localVector a vector fixed in the body.
	// @return the same vector expressed in world coordinates.
	b2Vec2 GetWorldVector(const b2Vec2& localVector) const { return b2Mul(m_xf, localVector); }

	// Get local point relative to the body's origin given a world point.
	// @param a point in world position.
	// @return the corresponding local point relative to the body's origin.
	b2Vec2 GetLocalPoint(const b2Vec2& worldPoint) const { return b2MulT(m_xf, worldPoint); }

	// Get local vector given a world vector.
	// @param a vector in world position.
	// @param return the corresponding local vector.
	b2Vec2 GetLocalVector(const b2Vec2& worldVector) const { return b2MulT(m_xf, worldVector);}

	// Get the world linear velocity of a world point attached to this body.
	// @param worldPoint a point in world coordinates.
	// @return the world velocity of a point.
	b2Vec2 GetLinearVelocityFromWorldPoint(const b2Vec2& worldPoint) const;

	// Get the world velocity of a local point.
	// @param localPoint a point in local coordinates.
	// @return the world velocity of a point.
	b2Vec2 GetLinearVelocityFromLocalPoint(const b2Vec2& localPoint) const;

	// Get the linear damping of the body.
	float32 GetLinearDamping() const { return m_linearDamping; }

	// Set the linear damping of the body.
	void SetLinearDamping(float32 linearDamping) { m_linearDamping = linearDamping; }

	// Get the angular damping of the body.
	float32 GetAngularDamping() const { return m_angularDamping; }

	// Set the angular damping of the body.
	void SetAngularDamping(float32 angularDamping) { m_angularDamping = angularDamping; }

	// Is this body treaded as bullet for CCD?
	bool IsBullet() const { return (m_flags&e_bulletFlag) == e_bulletFlag; }

	// Should this body be treated like bullet for CCD?
	void SetBullet(bool flag);

	// Is this body prevented from rotation.
	bool IsFixedRotation() const { return (m_flags&e_fixedRotationFlag) == e_fixedRotationFlag; }

	// Set if this body is prevent form rotation.
	void SetFixedRotation(bool fixed);

	// Is this body static ?
	bool IsStatic() const { return m_type == e_staticType; }

	// Make this body static.
	// Use SetMass and SetMassFromShapes to make body dymamic.
	void SetStatic();

	// Is this body dynamic?
	bool IsDynamic() const { return (m_type == e_dynamicType); }

	// Is this body frozen.
	bool IsFrozen() const { return (m_flags&e_frozenFlag) == e_frozenFlag; }

	// Is this body sleeping.
	bool IsSleeping() const { return (m_flags&e_sleepFlag) == e_sleepFlag; }

	// Is this body allowed to sleep?
	bool IsAllowSleep() const { return (m_flags&e_allowSleepFlag) == e_allowSleepFlag; }

	// You can disable sleeping on this body.
	void AllowSleeping(bool flag);

	// Wake this body so it will begin simulate.
	void WakeUp();

	// Put this body into sleep so it will stop simulating.
	// This also set the velocity to zero.
	void PutToSleep();

	// Get the list of fixtures attached to this body.
	b2Fixture* GetFixtureList() { return m_fixtureList; }

	// Get the list of fixtures attached to this body.
	const b2Fixture* GetFixtureList() const { return m_fixtureList; }

	// Get the list of all joints attached to this body.
	b2JointEdge* GetJointList() { return m_jointList; }

	// Get the list of all contacts attached to this body.
	// @warning this list changes during time step,and you may miss some collision
	// if you don't use b2ContactListener
	b2ContactEdge* GetContactList() { return m_contactList; }

	// Get the list of all controllers attached to this body.
	b2ControllerEdge* GetControllerList() { return m_controllerList; }

	// Get the next body in the world' body list.
	b2Body* GetNext() { return m_next; }

	// Get the next body in the world's body list.
	const b2Body* GetNext() const { return m_next; }

	// Get the userData pointer that was provided in the body's definition.
	void* GetUserData() const { return m_userData; }

	// Set the user data.use this to store your application apecific data.
	void SetUserData(void* data) { m_userData = data; }

	// Get the parent world of this body.
	b2World* GetWorld() { return m_world; }
private:
	friend class b2World;
	friend class b2Island;
	friend class b2ContactManager;
	friend class b2ContactSolver;

	friend class b2DistanceJoint;
	friend class b2GearJoint;
	friend class b2LineJoint;
	friend class b2MouseJoint;
	friend class b2PrismaticJoint;
	friend class b2PulleyJoint;
	friend class b2RevoluteJoint;
	friend class b2FixedJoint;

	friend class b2Controller;

	// m_flags
	enum
	{
		e_frozenFlag = 0x0002,
		e_islandFlag = 0x0004,
		e_sleepFlag = 0x0008,
		e_allowSleepFlag = 0x0010,
		e_bulletFlag = 0x0020,
		e_fixedRotationFlag = 0x0040,
	};

	// m_types
	enum
	{
		e_staticType,
		e_dynamicType,
		e_maxTypes,
	};

	b2Body(const b2BodyDef* bd, b2World* world);
	~b2Body();

	bool SynchronizeFixtures();

	void SynchronizeTransform();

	// This is used to prevent connected bodies from colliding.
	// It may die.depending on the collideConnected flag.
	bool IsConnected(const b2Body* other) const;

	void Advance(float32 t);

	uint16 m_flags;
	int16 m_type;

	int32 m_islandIndex;

	b2XForm m_xf;	// the body origin transform.
	b2Sweep m_sweep; // the swept motion for CCD.

	b2Vec2 m_linearVelocity;
	float32 m_angularVelocity;

	b2Vec2 m_force;
	float32 m_torque;

	b2World* m_world;
	b2Body* m_prev;
	b2Body* m_next;

	b2Fixture* m_fixtureList;
	int32 m_fixtureCount;

	b2JointEdge* m_jointList;
	b2ContactEdge* m_contactList;

	b2ControllerEdge* m_controllerList;

	float32 m_mass, m_invMass;
	float32 m_I, m_invI;

	float32 m_linearDamping;
	float32 m_angularDamping;

	float32 m_sleepTime;
	void* m_userData;
};

inline b2MassData b2Body::GetMassData()const
{
	b2MassData massData;
	massData.mass = m_mass;
	massData.I = m_I;
	massData.center = GetWorldCenter();
	return massData;
}

inline b2Vec2 b2Body::GetLinearVelocityFromWorldPoint(const b2Vec2& worldPoint) const
{
	return m_linearVelocity + b2Cross(m_angularVelocity, worldPoint - m_sweep.c);
}

inline b2Vec2 b2Body::GetLinearVelocityFromLocalPoint(const b2Vec2& localPoint) const
{
	return GetLinearVelocityFromWorldPoint(GetWorldPoint(localPoint));
}

inline void b2Body::SetBullet(bool flag)
{
	if(flag)
	{
		m_flags |= e_bulletFlag;
	}
	else
	{
		m_flags &= ~e_bulletFlag;
	}
}

inline void b2Body::SetFixedRotation(bool fixed)
{
	if(fixed)
	{
		m_angularVelocity = 0.0f;
		m_invI = 0.0f;
		m_flags |= e_fixedRotationFlag;
	}
	else
	{
		if(m_I > 0.0f)
		{
			// Recover m_invI from m_I.
			m_invI = 1.0f / m_I;
			m_flags &= e_fixedRotationFlag;
		}
	}
}

inline void b2Body::AllowSleeping(bool flag)
{
	if(flag)
	{
		m_flags |= e_allowSleepFlag;
	}
	else
	{
		m_flags &= ~e_allowSleepFlag;
		WakeUp();
	}
}

inline void b2Body::WakeUp()
{
	m_flags &= ~e_sleepFlag;
	m_sleepTime = 0.0f;
}

inline void b2Body::PutToSleep()
{
	m_flags |= e_sleepFlag;
	m_sleepTime = 0.0f;
	m_linearVelocity.SetZero();
	m_angularVelocity = 0.0f;
	m_force.SetZero();
	m_torque = 0.0f;
}

inline void b2Body::ApplyForce(const b2Vec2& force, const b2Vec2& point)
{
	if(IsSleeping())
	{
		WakeUp();
	}

	m_force += force;
	m_torque += b2Cross(point-m_sweep.c, force);
}

inline void b2Body::ApplyTorque(float32 torque)
{
	if(IsSleeping())
	{
		WakeUp();
	}
	m_torque += torque; 
}

inline void b2Body::ApplyImpulse(const b2Vec2& impulse, const b2Vec2& point)
{
	if(IsSleeping())
	{
		WakeUp();
	}

	m_linearVelocity += m_invMass * impulse;
	m_angularVelocity += m_invI * b2Cross(point-m_sweep.c, impulse);
}

inline void b2Body::SynchronizeTransform()
{
	m_xf.R.Set(m_sweep.a);
	m_xf.position = m_sweep.c - b2Mul(m_xf.R, m_sweep.localCenter);
}

#endif
